#include <string.h>
#include <stdlib.h>
#include "gkt_adc.h"
#include "gkt_os.h"
#include "gkt_debug.h"
#include "door_common.h"

#if defined(GKT_CONFIG_DOOR_LOCK_ENABLE) && GKT_CONFIG_DOOR_LOCK_ENABLE \
	&& defined(GKT_CONFIG_DOOR_LOCK_AUTOMATIC) && GKT_CONFIG_DOOR_LOCK_AUTOMATIC \
	&& (GKT_MODULE_LOC_FRONTBOARD == GKT_DOOR_LOCK_LOCATION)

#define DOOR_LOCK_DETECT_VOLTAGE_START_TIME_MS (100) //转动开始100ms后开始检测电压，过滤掉第一次电机动起来之后的ADC跳变
#define DOOR_LOCK_DETECT_VOLTAGE_LOOP_TIME_MS	(10)//循环检测ADC值的延时时间
#define DOOR_LOCK_DETECT_VOLTAGE_AVERAGE_COUNT (20)

#define DOOR_LOCK_DETECT_VOLTAGE_THRESHOLD_PERCENTAGE_FRONT		(0.1)//%10 主要用于去掉干扰
#define DOOR_LOCK_DETECT_VOLTAGE_THRESHOLD_PERCENTAGE_BEHIND	(0.15)//%15
#define DOOR_LOCK_DETECT_VOLTAGE_THRESHOLD_PERCENTAGE_STANDARD	(0.5)//堵转-正常转之后的百分比

#define DOOR_LOCK_DETECT_VOLTAGE_DEVIATION_MAX (30)  //前后电压偏差在这个范围内则认为电压稳定
#define DOOR_LOCK_DETECT_VOLTAGE_MAX (2048)//堵转电压最大不可能达到的值 单位mv
#define DOOR_LOCK_ADC_CONVERT_VOLT_MV(adc_value) ((1200)-(adc_value))
#define DOOR_LOCK_MOTOR_LOCKED_ROTOR_VOLTAGE_MV	(100)
#define DOOR_LOCK_MOTOR_LOCKED_ROTOR_VOLTAGE_JUMP_MAX_MV (150)
#define DOOR_LOCK_MOTOR_LOCKED_ROTOR_VOLTAGE_BEHIND_MV (180)

#define DOOR_LOCK_MOTOR_BRAKE_TIME_MS_MAX 200
#define DOOR_LOCK_MOTOR_BRAKE_TIME_MS_MIN 100

typedef struct _door_lock_auto_fb {
	int	init_ok;

	gkt_soft_timer_t	timer_id;
	gkt_thread_t	self_learn_task;

	int	need_rotate_half_circle;
	gkt_door_lock_body_type_e body_type;
	gkt_door_lock_motor_rotate_direction_e motor_rotate_direction;
	uint32_t motor_rotate_time;
	uint32_t double_close_motor_rotate_time;
	uint32_t motor_full_rotate_time;
	uint16_t voltage[GKT_DOOR_LOCK_MOTOR_ROTATE_DIRECTION_MAX][GKT_DOOR_LOCK_ROTOR_VOLTAGE_MAX];
} door_lock_auto_fb_s;
static door_lock_auto_fb_s s_door_lock_auto_fb;

static gkt_door_lock_st_e s_door_lock_status;

static void door_lock_motor_start_rotation(int direction)
{
	// TODO:
	if (GKT_DOOR_LOCK_MOTOR_ROTATE_DIRECTION_FORWARD == direction) {
		door_lock_motor_rotate_forward();
	}
	else {
		door_lock_motor_rotate_reverse();
	}
}

uint32_t gkt_door_lock_get_status(void)
{
	return s_door_lock_status;
}

int gkt_door_lock_open(void)
{
	int retval;

	gkt_info("door_lock_open enter state=%d\n", s_door_lock_status);

	if (!s_door_lock_auto_fb.init_ok)
		return GKT_ENOTINIT;

	if ((GKT_DOOR_LOCK_ST_CLOSED == s_door_lock_status)
		|| (GKT_DOOR_LOCK_ST_DL_CLOSED == s_door_lock_status)
		|| (GKT_DOOR_LOCK_ST_OPENED == s_door_lock_status)
		|| (GKT_DOOR_LOCK_ST_MAX == s_door_lock_status))
	{
		/* 1. start motor rotation */
		door_lock_motor_start_rotation(s_door_lock_auto_fb.motor_rotate_direction);

		/* 2. start the timer of detection of locked-rotor voltage */
		retval = gkt_soft_timer_start(s_door_lock_auto_fb.timer_id, 
					DOOR_LOCK_DETECT_VOLTAGE_START_TIME_MS);
		if (GKT_SUCCESS == retval) {
			s_door_lock_status = GKT_DOOR_LOCK_ST_OPENING;
			retval = GKT_EINPROCESS;
		}
		else {
			s_door_lock_status = GKT_DOOR_LOCK_ST_MAX;
			door_lock_motor_rotate_stop();
		}
	}
	else if (GKT_DOOR_LOCK_ST_OPENING == s_door_lock_status)
		retval = GKT_EINPROCESS;
	else
		retval = GKT_EPERMIT;

	return retval;
}

static int door_lock_close(void)
{
	int retval;

	gkt_info("door_lock_close enter direction=%d, state=%d\n", 
		s_door_lock_auto_fb.motor_rotate_direction, s_door_lock_status);

	if (GKT_DOOR_LOCK_ST_OPENED == s_door_lock_status
		|| (GKT_DOOR_LOCK_ST_MAX == s_door_lock_status)) 
	{
		/* 1. start motor open rotation */
		door_lock_motor_start_rotation(!s_door_lock_auto_fb.motor_rotate_direction);//取反即关
	
		/* 2. start the timer */
		retval = gkt_soft_timer_start(s_door_lock_auto_fb.timer_id, 
						s_door_lock_auto_fb.motor_rotate_time);
		if (GKT_SUCCESS == retval) {
			s_door_lock_status = GKT_DOOR_LOCK_ST_CLOSING;
			retval = GKT_EINPROCESS;
		}
		else {
			s_door_lock_status = GKT_DOOR_LOCK_ST_MAX;
			door_lock_motor_rotate_stop();
		}
	}
	else if ((GKT_DOOR_LOCK_ST_CLOSING == s_door_lock_status)
		|| (GKT_DOOR_LOCK_ST_DL_CLOSING == s_door_lock_status))
	{
		retval = GKT_EINPROCESS;
	}
	else if ((GKT_DOOR_LOCK_ST_CLOSED == s_door_lock_status)
		|| (GKT_DOOR_LOCK_ST_DL_CLOSED == s_door_lock_status)) 
	{
		retval = GKT_SUCCESS;
	}
	else
		retval = GKT_EPERMIT;

	return retval;
}

static int door_lock_double_lock(void)
{
	int retval;

	gkt_info("door_lock_double_lock enter direction=%d, state=%d\n", 
		s_door_lock_auto_fb.motor_rotate_direction, s_door_lock_status);

	if ((GKT_DOOR_LOCK_ST_OPENED == s_door_lock_status)
		|| (GKT_DOOR_LOCK_ST_CLOSED == s_door_lock_status)
		|| (GKT_DOOR_LOCK_ST_CLOSING == s_door_lock_status)
		|| (GKT_DOOR_LOCK_ST_DL_CLOSED == s_door_lock_status)
		|| (GKT_DOOR_LOCK_ST_MAX == s_door_lock_status))
	{
		s_door_lock_auto_fb.need_rotate_half_circle = 0;
		/* 1. start motor close rotation */
		door_lock_motor_start_rotation(!s_door_lock_auto_fb.motor_rotate_direction);
	
		/* 2. start the timer of detection of locked-rotor voltage ? */
		retval = gkt_soft_timer_start(s_door_lock_auto_fb.timer_id, 
					DOOR_LOCK_DETECT_VOLTAGE_START_TIME_MS);
		if (GKT_SUCCESS == retval) {
			s_door_lock_status = GKT_DOOR_LOCK_ST_DL_CLOSING;
			retval = GKT_EINPROCESS;
		}
		else {
			s_door_lock_status = GKT_DOOR_LOCK_ST_MAX;
			door_lock_motor_rotate_stop();
		}
	}
	else if (GKT_DOOR_LOCK_ST_DL_CLOSING == s_door_lock_status)
		retval = GKT_EINPROCESS;
	else 
		retval = GKT_EPERMIT;

	return retval;
}

int gkt_door_lock_close(int double_close)
{
	int status = GKT_EUNKNOWN;

	if (!s_door_lock_auto_fb.init_ok)
		return GKT_ENOTINIT;
	
	if (!double_close)
		status = door_lock_close();
	else
		status = door_lock_double_lock();
	return status;
}

void gkt_door_lock_reset(void)
{
	if (s_door_lock_auto_fb.init_ok) {
		gkt_printf("door_lock_reset: status(%u)\n", s_door_lock_status);

		gkt_soft_timer_stop(s_door_lock_auto_fb.timer_id);
		door_lock_motor_reset();
		s_door_lock_status = GKT_DOOR_LOCK_ST_MAX;
	}
}

uint32_t gkt_door_lock_get_timeout(gkt_door_lock_op_e op)
{
	uint32_t timeout;

	if ((GKT_DOOR_LOCK_OP_OPEN == op)
		|| (GKT_DOOR_LOCK_OP_DOUBLE_CLOSE == op)) 
	{
		timeout = s_door_lock_auto_fb.motor_full_rotate_time;
	}
	else if (GKT_DOOR_LOCK_OP_CLOSE == op)
		timeout = s_door_lock_auto_fb.motor_rotate_time;
	else if (GKT_DOOR_LOCK_OP_SELF_LEARN == op) {
		timeout = (s_door_lock_auto_fb.motor_full_rotate_time * 3)
			+ s_door_lock_auto_fb.motor_rotate_time;
	}
	else
		timeout = 0;

	return timeout;
}

int gkt_door_lock_set_motor_rotate_direction(
				gkt_door_lock_motor_rotate_direction_e direction)
{
	gkt_info("door_lock: motor_rotate_direction (%u -> %u)\n",
		s_door_lock_auto_fb.motor_rotate_direction, direction);

	if (direction < GKT_DOOR_LOCK_MOTOR_ROTATE_DIRECTION_MAX) {
		s_door_lock_auto_fb.motor_rotate_direction = direction;
		return GKT_SUCCESS;
	}

	return GKT_EPARAM;
}

uint32_t gkt_door_lock_get_motor_rotate_time(gkt_door_lock_motor_rotate_time_e type)
{
	if (GKT_DOOR_LOCK_MOTOR_ROTATE_TIME_CLOSE == type)
		return s_door_lock_auto_fb.motor_rotate_time;
	else if (GKT_DOOR_LOCK_MOTOR_ROTATE_TIME_FULL == type)
		return s_door_lock_auto_fb.motor_full_rotate_time;
	else if (GKT_DOOR_LOCK_MOTOR_ROTATE_TIME_DOUBLE_CLOSE == type)
		return s_door_lock_auto_fb.double_close_motor_rotate_time;
	return 0;
}

int gkt_door_lock_set_motor_rotate_time(gkt_door_lock_motor_rotate_time_e type,
	uint32_t time_ms)
{
	gkt_info("door_lock_set_motor_rotate_time: type(%d), time_ms(%u)\n", type, time_ms);

	if (!time_ms || (type >= GKT_DOOR_LOCK_MOTOR_ROTATE_TIME_MAX))
		return GKT_EPARAM;

	if (GKT_DOOR_LOCK_MOTOR_ROTATE_TIME_CLOSE == type)
		s_door_lock_auto_fb.motor_rotate_time = time_ms;
	else if (GKT_DOOR_LOCK_MOTOR_ROTATE_TIME_FULL == type)
		s_door_lock_auto_fb.motor_full_rotate_time = time_ms;
	else if (GKT_DOOR_LOCK_MOTOR_ROTATE_TIME_DOUBLE_CLOSE == type)
		s_door_lock_auto_fb.double_close_motor_rotate_time = time_ms;
	return GKT_SUCCESS;
}

int gkt_door_lock_get_motor_locked_rotor_voltage(
			uint16_t voltage[GKT_DOOR_LOCK_MOTOR_ROTATE_DIRECTION_MAX][GKT_DOOR_LOCK_ROTOR_VOLTAGE_MAX])
{	
	int i, j;

	if (NULL == voltage)
		return GKT_EPARAM;

	for (i=0; i<GKT_DOOR_LOCK_MOTOR_ROTATE_DIRECTION_MAX; i++) {
		for (j=0; j<GKT_DOOR_LOCK_ROTOR_VOLTAGE_MAX; j++) {
			voltage[i][j] = s_door_lock_auto_fb.voltage[i][j];
		}
	}
	
	return GKT_SUCCESS;
}

int gkt_door_lock_set_motor_locked_rotor_voltage(
			const uint16_t voltage[GKT_DOOR_LOCK_MOTOR_ROTATE_DIRECTION_MAX][GKT_DOOR_LOCK_ROTOR_VOLTAGE_MAX])
{
	int i, j;

	if ((NULL == voltage) ||
		(0 == voltage[GKT_DOOR_LOCK_MOTOR_ROTATE_DIRECTION_FORWARD][GKT_DOOR_LOCK_ROTOR_VOLTAGE_TORSION]) || 
		(0 == voltage[GKT_DOOR_LOCK_MOTOR_ROTATE_DIRECTION_REVERSE][GKT_DOOR_LOCK_ROTOR_VOLTAGE_TORSION]))
		return GKT_EPARAM;

	for (i=0; i<GKT_DOOR_LOCK_MOTOR_ROTATE_DIRECTION_MAX; i++) {
		for (j=0; j<GKT_DOOR_LOCK_ROTOR_VOLTAGE_MAX; j++) {
			s_door_lock_auto_fb.voltage[i][j] = voltage[i][j];
		}
	}
	
	return GKT_SUCCESS;
}

gkt_door_lock_body_type_e gkt_door_lock_get_body_type(void)
{
	return s_door_lock_auto_fb.body_type;
}

int gkt_door_lock_set_motor_torsion(gkt_door_lock_motor_torsion_gear_e gear)
{
	uint32_t volt[2]; 
	int i;

	if (gear >= GKT_DOOR_LOCK_MOTOR_TORSION_GEAR_MAX)
		return GKT_EPARAM;

	if (0 == s_door_lock_auto_fb.voltage[0][GKT_DOOR_LOCK_ROTOR_VOLTAGE_BEHIND])
		return GKT_EPERMIT; 

	for (i=0; i<GKT_DOOR_LOCK_MOTOR_ROTATE_DIRECTION_MAX; i++) {  
		volt[0] = s_door_lock_auto_fb.voltage[i][GKT_DOOR_LOCK_ROTOR_VOLTAGE_STANDARD];
		volt[1] = s_door_lock_auto_fb.voltage[i][GKT_DOOR_LOCK_ROTOR_VOLTAGE_BEHIND];
		if (volt[1] >= volt[0]) {
			s_door_lock_auto_fb.voltage[i][GKT_DOOR_LOCK_ROTOR_VOLTAGE_TORSION] =
				volt[0] + gear*((volt[1]-volt[0])/(GKT_DOOR_LOCK_MOTOR_TORSION_GEAR_MAX-1));
		}
		else
			return GKT_EPARAM;
	}

	return GKT_SUCCESS;
}

#ifndef GKT_CONFIG_ADC_MOTOR_LOCKED_CHANNEL
#error "no define adc channel!"
#endif
static uint16_t door_lock_get_adc_value(void)
{
	uint32_t adc_value = 0;
	uint8_t dev_id = 0;
	gkt_adc_attr_s adc_attr;
	int retval, value;

	adc_attr.channel = GKT_CONFIG_ADC_MOTOR_LOCKED_CHANNEL;
	adc_attr.mode = ADC_GPIO;//ADC_VINLPM
	adc_attr.scale_type = ADC_Scale_1200Mv;
	retval = gkt_adc_config(dev_id, &adc_attr);
	if (retval != GKT_SUCCESS)
		gkt_error("adc config err=%d\n",retval);

	retval = gkt_adc_start(0, &value);
	if (retval == GKT_SUCCESS) {
		adc_value = (uint32_t)value;
	}
	//gkt_printf("door_lock_get_adc_value: adc_value=%d\n", adc_value);
	return adc_value;//DOOR_LOCK_ADC_CONVERT_VOLT_MV(adc_value);
}

static int door_lock_detect_voltage_reach_threshold(uint32_t volt_value_threshold)
{
	uint32_t vol = 0;
	static uint32_t detect_voltage_times = 0;
	static uint32_t detect_success_counts = 0;

	vol = door_lock_get_adc_value();
	__ISB();
	vol = door_lock_get_adc_value();

	if (0 == detect_voltage_times) {
		detect_success_counts = 0;
		detect_voltage_times = gkt_systime_ms();
		gkt_trace("detect_voltage time:%dms, vol:%dmv\n", detect_voltage_times, vol);
	}

	if (vol > volt_value_threshold) {
		detect_success_counts++;
		gkt_trace("detect_voltage count:%d, vol:%dmv, detect_success_counts:%d\n",
					detect_voltage_times, vol, detect_success_counts);
		if (15 == detect_success_counts) {//15次检测都大于堵转阈值则认为确实发生了堵转
			detect_voltage_times = 0;
			return GKT_SUCCESS;
		}
	}
	else {
		if (detect_success_counts > 0)
			detect_success_counts--;
	}
	if ((gkt_systime_ms() - detect_voltage_times) < (s_door_lock_auto_fb.motor_full_rotate_time)) {
		gkt_soft_timer_start(s_door_lock_auto_fb.timer_id, DOOR_LOCK_DETECT_VOLTAGE_LOOP_TIME_MS);
		return GKT_EBUSY;
	}
	else {
		gkt_printf("detect_voltage: timeout=%dms\n", gkt_systime_ms() - detect_voltage_times);
		detect_voltage_times = 0;
		return GKT_ETIMEOUT;
	}
}

/*Timer is not used here, 
* because the motor self-learning when the front plate and the back plate 
  will not have excess action
*/
static int door_lock_motor_self_learn(gkt_door_lock_op_e op)
{
	uint32_t stop_rotate_volt = 0;
	uint32_t rotating_volt[4];
	uint32_t adc_value_timeout_ms = 0, temp_timeout_ms=0;
	uint32_t i;
	uint32_t locked_counts = 0;

	gkt_printf("door_lock_motor_self_learn: op(%u)\n", op);

	memset(rotating_volt, 0, sizeof(rotating_volt));

	if (op >= GKT_DOOR_LOCK_OP_MAX)
		return GKT_EPARAM;

	door_lock_motor_rotate_brake();
	gkt_thread_sleep(DOOR_LOCK_MOTOR_BRAKE_TIME_MS_MAX);
	stop_rotate_volt = door_lock_get_adc_value();
	gkt_printf("stop volt:%dmV\r\n", stop_rotate_volt);

	if (GKT_DOOR_LOCK_OP_OPEN == op)
		door_lock_motor_start_rotation(s_door_lock_auto_fb.motor_rotate_direction);
	else if (GKT_DOOR_LOCK_OP_DOUBLE_CLOSE == op)
		door_lock_motor_start_rotation(!s_door_lock_auto_fb.motor_rotate_direction);
	else if (GKT_DOOR_LOCK_OP_CLOSE == op) {
		door_lock_motor_start_rotation(!s_door_lock_auto_fb.motor_rotate_direction);
		gkt_thread_sleep(s_door_lock_auto_fb.motor_rotate_time);
		door_lock_motor_rotate_brake();
		gkt_thread_sleep(DOOR_LOCK_MOTOR_BRAKE_TIME_MS_MIN);
		door_lock_motor_rotate_stop();
		return GKT_SUCCESS;
	}

	gkt_thread_sleep(100);
	gkt_printf("normal volt:%dmV\r\n", door_lock_get_adc_value());

	adc_value_timeout_ms = gkt_systime_ms();
	while (1) {
		rotating_volt[0] = door_lock_get_adc_value();
		gkt_delay_ms(DOOR_LOCK_DETECT_VOLTAGE_LOOP_TIME_MS);
		gkt_printf("current volt:%dmv\n", rotating_volt[0]);

		if (abs(rotating_volt[0]-stop_rotate_volt) >= DOOR_LOCK_MOTOR_LOCKED_ROTOR_VOLTAGE_MV)
		{
			gkt_printf("***********enter greater than 100mv**********\n");
			temp_timeout_ms = gkt_systime_ms();
			locked_counts = 0;
			while (1) {
				gkt_delay_ms(DOOR_LOCK_DETECT_VOLTAGE_LOOP_TIME_MS);
				rotating_volt[0] = door_lock_get_adc_value();
				gkt_printf("current volt:%dmv\n", rotating_volt[0]);
				rotating_volt[1] = abs(rotating_volt[0]-stop_rotate_volt);
				//1. 若进来后adc变化率又小于100mv就退出到大循环
				if (rotating_volt[1] < DOOR_LOCK_MOTOR_LOCKED_ROTOR_VOLTAGE_MV) {
					gkt_printf("current volt less than 100mv, continue detect\n");
					goto TIME_OUT_END;
				}//2. 判决流程中连续5次大于150mv，判定为堵转
				else if (rotating_volt[1] >= DOOR_LOCK_MOTOR_LOCKED_ROTOR_VOLTAGE_JUMP_MAX_MV) {
					locked_counts++;
					gkt_printf("current volt greater than 150mv, count=%d\n", locked_counts);
					if (locked_counts == 5)
						break;
				}
				else
					locked_counts = 0;

				if ((gkt_systime_ms() - temp_timeout_ms) > 500) {
					gkt_printf("goto time out loop\n");
					goto TIME_OUT_END;
				}
			}
			//发生跳变之后延时50ms等电压稳定开始取值进行比较
			gkt_printf("***********enter calculate average value**********\n");
			gkt_thread_sleep(50);
			rotating_volt[0] = door_lock_get_adc_value();
			gkt_printf("current volt:%dmv\n", rotating_volt[0]);
			for (i=1; i<4; i++) {
				gkt_delay_ms(DOOR_LOCK_DETECT_VOLTAGE_LOOP_TIME_MS);
				rotating_volt[i] = door_lock_get_adc_value();
				gkt_printf("current volt:%dmv\n", rotating_volt[i]);
			}
			//if (abs((rotating_volt[1]+rotating_volt[2]+rotating_volt[3])/3 - rotating_volt[0]) <= DOOR_LOCK_DETECT_VOLTAGE_DEVIATION_MAX)
			{
				door_lock_motor_rotate_brake();
				rotating_volt[0] = (rotating_volt[1]+rotating_volt[2]+rotating_volt[3])/3; //堵转电压
				rotating_volt[1] = stop_rotate_volt + (rotating_volt[0]-stop_rotate_volt)*DOOR_LOCK_DETECT_VOLTAGE_THRESHOLD_PERCENTAGE_FRONT;//前门限暂时不用
				
				i = (GKT_DOOR_LOCK_OP_OPEN == op) ? (s_door_lock_auto_fb.motor_rotate_direction) : (!s_door_lock_auto_fb.motor_rotate_direction);

				s_door_lock_auto_fb.voltage[i][GKT_DOOR_LOCK_ROTOR_VOLTAGE_BEHIND] = 
					rotating_volt[0] - (rotating_volt[0]-stop_rotate_volt)*DOOR_LOCK_DETECT_VOLTAGE_THRESHOLD_PERCENTAGE_BEHIND;//后门限

				s_door_lock_auto_fb.voltage[i][GKT_DOOR_LOCK_ROTOR_VOLTAGE_STANDARD] = 
					s_door_lock_auto_fb.voltage[i][GKT_DOOR_LOCK_ROTOR_VOLTAGE_TORSION] = 
					stop_rotate_volt + (rotating_volt[0]-stop_rotate_volt)*DOOR_LOCK_DETECT_VOLTAGE_THRESHOLD_PERCENTAGE_STANDARD;//初始阈值

				gkt_printf("locked volt:%dmV, %dmV-%dmV-%dmV\r\n", rotating_volt[0], rotating_volt[1], 
					s_door_lock_auto_fb.voltage[i][0], s_door_lock_auto_fb.voltage[i][1]);

				return GKT_SUCCESS;
			}
		}
TIME_OUT_END:
		if ((gkt_systime_ms() - adc_value_timeout_ms) > (GKT_DOOR_LOCK_MOTOR_FULL_ROTATE_TIME_MS)) {
			door_lock_motor_rotate_brake();
			gkt_printf("door_lock_motor_self_learn timeout, time=%d\n", \
				gkt_systime_ms() - adc_value_timeout_ms);
			return GKT_ETIMEOUT;
		}
	}

	return GKT_SUCCESS;
}

#define DOOR_LOCK_LEARN_SUCCESS_OP_OPEN 		(1U << GKT_DOOR_LOCK_OP_OPEN)
#define DOOR_LOCK_LEARN_SUCCESS_OP_DOUBLE_CLOSE (1U << GKT_DOOR_LOCK_OP_DOUBLE_CLOSE)
static void door_lock_motor_self_learn_handler(void)
{
	uint32_t learn_success_bits = 0;
	int retval;
	int i;

	retval = door_lock_motor_self_learn(GKT_DOOR_LOCK_OP_OPEN);
	if (retval == GKT_SUCCESS) {
		gkt_info("door_lock_motor_self_learn: 0-OPEN - SUCCEED \n");
	}

	retval = door_lock_motor_self_learn(GKT_DOOR_LOCK_OP_DOUBLE_CLOSE);
	if (retval == GKT_SUCCESS) {
		gkt_info("door_lock_motor_self_learn: DOUBLE_CLOSE - SUCCEED \n");
		learn_success_bits |= DOOR_LOCK_LEARN_SUCCESS_OP_DOUBLE_CLOSE;
	}

	retval = door_lock_motor_self_learn(GKT_DOOR_LOCK_OP_OPEN);
	if (retval == GKT_SUCCESS) {
		gkt_info("door_lock_motor_self_learn: 1-OPEN - SUCCEED \n");
		learn_success_bits |= DOOR_LOCK_LEARN_SUCCESS_OP_OPEN;
	}
	gkt_thread_sleep(DOOR_LOCK_MOTOR_BRAKE_TIME_MS_MAX);
	door_lock_motor_rotate_stop();

	if (!(learn_success_bits & DOOR_LOCK_LEARN_SUCCESS_OP_OPEN) &&
		!(learn_success_bits & DOOR_LOCK_LEARN_SUCCESS_OP_DOUBLE_CLOSE))
	{
		gkt_error("door_lock_motor_self_learn: 1-OPEN - DOUBLE_CLOSE - failed\n");
		door_inner_notify(GKT_DOOR_NOTIFY_LOCK_STATUS, GKT_DOOR_LOCK_ST_SELF_LEARN_FAILED);
		goto self_learn_complete;
	}
	if (learn_success_bits & DOOR_LOCK_LEARN_SUCCESS_OP_DOUBLE_CLOSE) {
		if (s_door_lock_auto_fb.voltage[!s_door_lock_auto_fb.motor_rotate_direction][0] >= DOOR_LOCK_DETECT_VOLTAGE_MAX ||
			s_door_lock_auto_fb.voltage[!s_door_lock_auto_fb.motor_rotate_direction][1] >= DOOR_LOCK_DETECT_VOLTAGE_MAX)
		{
			gkt_error("door_lock_motor_self_learn: voltage invalid!\n");
			door_inner_notify(GKT_DOOR_NOTIFY_LOCK_STATUS, GKT_DOOR_LOCK_ST_SELF_LEARN_FAILED);
			goto self_learn_complete;
		}
		if (!(learn_success_bits & DOOR_LOCK_LEARN_SUCCESS_OP_OPEN)) {
			s_door_lock_auto_fb.body_type = GKT_DOOR_LOCK_BODY_TYPE_SELF_POP;
			for (i=0; i<GKT_DOOR_LOCK_ROTOR_VOLTAGE_MAX; i++) {
				s_door_lock_auto_fb.voltage[s_door_lock_auto_fb.motor_rotate_direction][i] =
					s_door_lock_auto_fb.voltage[!s_door_lock_auto_fb.motor_rotate_direction][i];
			}
		}
	}
	if (learn_success_bits & DOOR_LOCK_LEARN_SUCCESS_OP_OPEN) {
		if (s_door_lock_auto_fb.voltage[s_door_lock_auto_fb.motor_rotate_direction][0] >= DOOR_LOCK_DETECT_VOLTAGE_MAX ||
			s_door_lock_auto_fb.voltage[s_door_lock_auto_fb.motor_rotate_direction][1] >= DOOR_LOCK_DETECT_VOLTAGE_MAX)
		{
			gkt_error("door_lock_motor_self_learn: voltage invalid!\n");
			door_inner_notify(GKT_DOOR_NOTIFY_LOCK_STATUS, GKT_DOOR_LOCK_ST_SELF_LEARN_FAILED);
			goto self_learn_complete;
		}
		if (!(learn_success_bits & DOOR_LOCK_LEARN_SUCCESS_OP_DOUBLE_CLOSE)) {
			s_door_lock_auto_fb.body_type = GKT_DOOR_LOCK_BODY_TYPE_SELF_POP;
			for (i=0; i<GKT_DOOR_LOCK_ROTOR_VOLTAGE_MAX; i++) {
				s_door_lock_auto_fb.voltage[!s_door_lock_auto_fb.motor_rotate_direction][i] =
					s_door_lock_auto_fb.voltage[s_door_lock_auto_fb.motor_rotate_direction][i];
			}
		}
	}
	//正转和反转仅保留电压较大的那一组值
	if (s_door_lock_auto_fb.voltage[0][0] > s_door_lock_auto_fb.voltage[1][0]) {
		for (i=0; i<GKT_DOOR_LOCK_ROTOR_VOLTAGE_MAX; i++)
			s_door_lock_auto_fb.voltage[1][i] = s_door_lock_auto_fb.voltage[0][i];
	}
	else {
		for (i=0; i<GKT_DOOR_LOCK_ROTOR_VOLTAGE_MAX; i++)
			s_door_lock_auto_fb.voltage[0][i] = s_door_lock_auto_fb.voltage[1][i];
	}
	door_inner_notify(GKT_DOOR_NOTIFY_LOCK_STATUS, GKT_DOOR_LOCK_ST_SELF_LEARN_COMPLETE);

self_learn_complete:
	door_lock_motor_self_learn(GKT_DOOR_LOCK_OP_CLOSE);
	s_door_lock_status = GKT_DOOR_LOCK_ST_CLOSED;
}

static void door_lock_timer_callback(void *p)
{
	uint32_t volt_threshold;
	int res = GKT_SUCCESS;
	static int s_motor_brake_flag = 0;

	gkt_trace("door_lock_timer_callback :s_door_lock_status = %d\n", s_door_lock_status);

	if (GKT_DOOR_LOCK_ST_OPENING == s_door_lock_status) {
		/* detection of locked-rotor voltage */
		// TODO:
		/* if not reach, set timer to detect again & retrun */
		volt_threshold = s_door_lock_auto_fb.voltage[s_door_lock_auto_fb.motor_rotate_direction][GKT_DOOR_LOCK_ROTOR_VOLTAGE_TORSION];
		res = door_lock_detect_voltage_reach_threshold(volt_threshold);
		if (GKT_SUCCESS == res || GKT_ETIMEOUT == res) {
			/* if reach the locked-rotor voltage, open complete */
			s_door_lock_status = GKT_DOOR_LOCK_ST_OPENED;
			door_lock_motor_rotate_brake();
			door_inner_notify(GKT_DOOR_NOTIFY_LOCK_STATUS, GKT_DOOR_LOCK_ST_OPENED);
		}
	}
	else if (GKT_DOOR_LOCK_ST_CLOSING == s_door_lock_status) {
		if (s_motor_brake_flag == 0) {
			door_lock_motor_rotate_brake();
			res = gkt_soft_timer_start(s_door_lock_auto_fb.timer_id, DOOR_LOCK_MOTOR_BRAKE_TIME_MS_MIN);
			if (GKT_SUCCESS == res)
				s_motor_brake_flag = 1;
			else {
				s_door_lock_status = GKT_DOOR_LOCK_ST_CLOSED;
				door_lock_motor_rotate_stop();
				door_inner_notify(GKT_DOOR_NOTIFY_LOCK_STATUS, s_door_lock_status);
			}
		}
		else {
			s_motor_brake_flag = 0;
			s_door_lock_status = GKT_DOOR_LOCK_ST_CLOSED;
			door_lock_motor_rotate_stop();
			door_inner_notify(GKT_DOOR_NOTIFY_LOCK_STATUS, s_door_lock_status);
		}
	}
	else if (GKT_DOOR_LOCK_ST_DL_CLOSING == s_door_lock_status) {
		if (s_door_lock_auto_fb.need_rotate_half_circle == 0) {
			/* detection of locked-rotor voltage */
			// TODO:
			/* if not reach, set timer to detect again & retrun */
			volt_threshold = s_door_lock_auto_fb.voltage[!s_door_lock_auto_fb.motor_rotate_direction][GKT_DOOR_LOCK_ROTOR_VOLTAGE_TORSION];
			res = door_lock_detect_voltage_reach_threshold(volt_threshold);
			if (GKT_SUCCESS == res || GKT_ETIMEOUT == res) {
				door_lock_motor_rotate_brake();

				res = gkt_soft_timer_start(s_door_lock_auto_fb.timer_id, 1000);
				if (GKT_SUCCESS == res)
					s_door_lock_auto_fb.need_rotate_half_circle = 1;
				else {
					s_door_lock_status = GKT_DOOR_LOCK_ST_DL_CLOSED;
					door_lock_motor_rotate_stop();
					door_inner_notify(GKT_DOOR_NOTIFY_LOCK_STATUS, s_door_lock_status);
				}
			}
		}
		else if (s_door_lock_auto_fb.need_rotate_half_circle == 1) {
			door_lock_motor_start_rotation(s_door_lock_auto_fb.motor_rotate_direction);

			res = gkt_soft_timer_start(s_door_lock_auto_fb.timer_id,
						s_door_lock_auto_fb.double_close_motor_rotate_time);
			if (GKT_SUCCESS == res)
				s_door_lock_auto_fb.need_rotate_half_circle = 2;
			else {
				s_door_lock_auto_fb.need_rotate_half_circle = 0;
				s_door_lock_status = GKT_DOOR_LOCK_ST_DL_CLOSED;
				door_lock_motor_rotate_stop();
				door_inner_notify(GKT_DOOR_NOTIFY_LOCK_STATUS, s_door_lock_status);
			}
		}
		else {
			if (s_motor_brake_flag == 0) {
				door_lock_motor_rotate_brake();
				res = gkt_soft_timer_start(s_door_lock_auto_fb.timer_id, DOOR_LOCK_MOTOR_BRAKE_TIME_MS_MIN);
				if (GKT_SUCCESS == res)
					s_motor_brake_flag = 1;
				else {
					s_door_lock_auto_fb.need_rotate_half_circle = 0;
					s_door_lock_status = GKT_DOOR_LOCK_ST_DL_CLOSED;
					door_lock_motor_rotate_stop();
					door_inner_notify(GKT_DOOR_NOTIFY_LOCK_STATUS, s_door_lock_status);
				}
			}
			else {
				s_motor_brake_flag = 0;
				s_door_lock_auto_fb.need_rotate_half_circle = 0;
				s_door_lock_status = GKT_DOOR_LOCK_ST_DL_CLOSED;
				door_lock_motor_rotate_stop();
				door_inner_notify(GKT_DOOR_NOTIFY_LOCK_STATUS, s_door_lock_status);
			}
		}
	}
}

static void door_lock_motor_self_learn_task(void *arguments)
{
	while (1) {
		gkt_thread_sleep(100);
		if (GKT_DOOR_LOCK_ST_SELF_LEARNING == s_door_lock_status) {
			door_lock_motor_reset();
			door_lock_motor_self_learn_handler();
		}
	}
}

int gkt_door_lock_motor_self_learn(void)
{
	int retval;

	if (!s_door_lock_auto_fb.init_ok)
		return GKT_ENOTINIT;

	gkt_info("door_lock_motor_self_learn: status(%u)\n", s_door_lock_status);

	if (GKT_DOOR_LOCK_ST_OPENING != s_door_lock_status &&
		GKT_DOOR_LOCK_ST_CLOSING != s_door_lock_status &&
		GKT_DOOR_LOCK_ST_DL_CLOSING != s_door_lock_status &&
		GKT_DOOR_LOCK_ST_SELF_LEARNING != s_door_lock_status) 
	{
		if (!s_door_lock_auto_fb.self_learn_task) {
			s_door_lock_auto_fb.self_learn_task = 
				gkt_thread_create(door_lock_motor_self_learn_task,
						NULL, GKT_THREAD_PRIORITY_NORMAL,
						0x1000, NULL);
			if (!s_door_lock_auto_fb.self_learn_task) {
				gkt_error("door_lock_motor_self_learn: create task failed!\n");
				return GKT_ECREATE;
			}
		}
		s_door_lock_status = GKT_DOOR_LOCK_ST_SELF_LEARNING;

		retval = GKT_EINPROCESS;
	}
	else
		retval = GKT_EPERMIT;

	return retval;
}
void gkt_door_lock_standby(void)
{
	//door_lock_motor_standby();
}
int door_lock_init(void)
{
	int i, retval;
	uint16_t default_volt, default_volt_behind;

	s_door_lock_status = GKT_DOOR_LOCK_ST_MAX;

	s_door_lock_auto_fb.init_ok = 0;
	s_door_lock_auto_fb.need_rotate_half_circle = 0;
	s_door_lock_auto_fb.body_type = GKT_DOOR_LOCK_BODY_TYPE_NORMAL;
	s_door_lock_auto_fb.motor_rotate_direction = GKT_DOOR_LOCK_MOTOR_ROTATE_DIRECTION;
	s_door_lock_auto_fb.motor_rotate_time = GKT_DOOR_LOCK_MOTOR_ROTATE_TIME_MS;
	s_door_lock_auto_fb.double_close_motor_rotate_time = GKT_DOOR_LOCK_DOUBLE_CLOSE_MOTOR_ROTATE_TIME_MS;
	s_door_lock_auto_fb.motor_full_rotate_time = GKT_DOOR_LOCK_MOTOR_FULL_ROTATE_TIME_MS;
	
	s_door_lock_auto_fb.timer_id = gkt_soft_timer_create(GKT_SOFT_TIMER_ONCE, 
						door_lock_timer_callback, NULL);
	if (!s_door_lock_auto_fb.timer_id) {
		gkt_error("door_lock: create soft timer failed!\n");
		return GKT_ECREATE;
	}
	s_door_lock_auto_fb.self_learn_task = NULL;

	retval = door_lock_motor_init();
	if (retval != GKT_SUCCESS)
		return retval;

	default_volt = door_lock_get_adc_value() + DOOR_LOCK_MOTOR_LOCKED_ROTOR_VOLTAGE_MV;
	default_volt_behind = door_lock_get_adc_value() + DOOR_LOCK_MOTOR_LOCKED_ROTOR_VOLTAGE_BEHIND_MV;
	for (i=0; i<GKT_DOOR_LOCK_MOTOR_ROTATE_DIRECTION_MAX; i++) {
		s_door_lock_auto_fb.voltage[i][GKT_DOOR_LOCK_ROTOR_VOLTAGE_BEHIND] = default_volt_behind;
		s_door_lock_auto_fb.voltage[i][GKT_DOOR_LOCK_ROTOR_VOLTAGE_STANDARD] = default_volt;
		s_door_lock_auto_fb.voltage[i][GKT_DOOR_LOCK_ROTOR_VOLTAGE_TORSION] = default_volt;
	}

	s_door_lock_auto_fb.init_ok = 1;

	return GKT_SUCCESS;
}

#endif

